# This is a sample Python script.
import struct
import serial

# Press Shift+F10 to execute it or replace it with your code.
# Press Double Shift to search everywhere for classes, files, tool windows, actions, and settings.


serial_port = serial.Serial(
    port="/dev/ttyTHS1",
    baudrate=115200,
    bytesize=serial.EIGHTBITS,
    parity=serial.PARITY_NONE,
    stopbits=serial.STOPBITS_ONE,
)


def move(x, y):
    x = limit(x)
    y = limit(y)
    # print(x)

    frame = build_frame(x, y)

    # print(frame.hex())

    serial_port.write(frame)

    return frame


I16_MAX = 32767
I16_MIN = -32768


def limit(v):
    return max(min(v, I16_MAX), I16_MIN)


def crc8(data):
    crc = 0
    for b in data:
        crc ^= b
        for _ in range(8):
            if crc & 0x80:
                crc = (crc << 1) ^ 0x07
            else:
                crc <<= 1
            crc &= 0xff
    return crc


def build_frame(x, y):
    # Frame Format
    # ------------------------------------------
    # | 0x5a | x(l) | x(h) | y(l) | y(h) | dist(l) | dist(h) | crc |
    # ------------------------------------------
    frame = bytearray([0x5a])  # Frame Header

    # print(x, y)  # For Debug

    frame.extend(struct.pack('<h', x))
    frame.extend(struct.pack('<h', y))

    crc_value = crc8(frame[1:])  # Calculate crc for x and y
    frame.append(crc_value)

    return frame


# Press the green button in the gutter to run the script.
if __name__ == '__main__':
    # For Debug
    # print(build_frame(-1, -2).hex())
    # print(build_frame(100, 32).hex())

    print('''Usage example:
import gcg_interface
gcg_interface.move(100, 100)''')

# See PyCharm help at https://www.jetbrains.com/help/pycharm/
